package upm.etsit.gsi.lejos.mazesolver.behaviours;

import upm.etsit.gsi.lejos.mazesolver.DataExchange;
import lejos.nxt.LCD;
import lejos.robotics.navigation.TachoPilot;
import lejos.robotics.subsumption.Behavior;
import lejos.nxt.Motor;

/**
 * Clase Exploration
 *
 * @author jrubia
 */
public class Exploration implements Behavior {

    /**
     * Piloto que controla el movimiento del robot.
     */
    private TachoPilot pilot;
    /**
     * Intercambiador de datos.
     */
    private DataExchange DE;
    /**
     * Mapeador.
     */
    private Mapper mapper;

    /**
     * Constructor con parámetros
     *
     * @param pilot
     * @param DE
     * @param mapper);
     */
    public Exploration(TachoPilot pilot, DataExchange DE, Mapper mapper) {
        this.pilot = pilot;
        this.DE = DE;
        this.mapper = mapper;
    }

    public boolean takeControl() {
        return DE.isExploracion();
    }

    public void action() {

    	this.mapper.map();

        boolean[] directions = new boolean[3];

        Motor.B.rotateTo(90);
        directions[0] = DE.getDistance() > DE.getDIS_SEG();
        Motor.B.rotateTo(0);
        directions[1] = DE.getDistance() > DE.getDIS_SEG();
        Motor.B.rotateTo(-90);
        directions[2] = DE.getDistance() > DE.getDIS_SEG();
        Motor.B.rotateTo(0);


        if (directions[0] && !tryLeft()) {
            LCD.drawString("a" , 0, 1);
            this.pilot.rotate(90);
            this.mapper.rotate(90);            
            DE.setExploration(false);

        } else if (directions[1] && !tryCenter()) {
LCD.drawString("b" , 0, 1);
            DE.setExploration(false);

        } else if (directions[2] && !tryRigth()) {
            LCD.drawString("c" , 0, 1);
            this.pilot.rotate(-90);
            this.mapper.rotate(-90);
            DE.setExploration(false);

        } else if (directions[0]) {
            LCD.drawString("d" , 0, 1);
            this.pilot.rotate(90);
            this.mapper.rotate(90);

        } else if (directions[1]) {
            LCD.drawString("e" , 0, 1);
        } else if(directions[2]) {
            this.pilot.rotate(-90);
            this.mapper.rotate(-90);
        }

        if(DE.isExploracion()) {
        this.pilot.travel(DE.getDIS_SEG()*10, true);

        while (pilot.isMoving()) {
            if (DE.getDistance() < 5) {
                pilot.stop();
            }
        }

        this.mapper.advance();
        }

    }

    public void suppress() {
        DE.setDirection(0);
    }

    private boolean tryLeft() {
        switch (mapper.getDirection()) {
            case 0:
                if (mapper.testAlreadyStay(mapper.getX(), mapper.getY() + 1)) {
                    return true;
                } else {
                    return false;
                }
            case 1:
                if (mapper.testAlreadyStay(mapper.getX() - 1, mapper.getY())) {
                    return true;
                } else {
                    return false;
                }
            case 2:
                if (mapper.testAlreadyStay(mapper.getX(), mapper.getY() - 1)) {
                    return true;
                } else {
                    return false;
                }
            case 3:
                if (mapper.testAlreadyStay(mapper.getX() + 1, mapper.getY())) {
                    return true;
                } else {
                    return false;
                }
        }
        return false;
    }

    public boolean tryCenter() {
        switch (mapper.getDirection()) {
            case 0:
                if (mapper.testAlreadyStay(mapper.getX() + 1, mapper.getY())) {
                    return true;
                } else {
                    return false;
                }
            case 1:
                if (mapper.testAlreadyStay(mapper.getX(), mapper.getY() + 1)) {
                    return true;
                } else {
                    return false;
                }
            case 2:
                if (mapper.testAlreadyStay(mapper.getX() - 1, mapper.getY())) {
                    return true;
                } else {
                    return false;
                }
            case 3:
                if (mapper.testAlreadyStay(mapper.getX(), mapper.getY() - 1)) {
                    return true;
                } else {
                    return false;
                }
        }
        return false;
    }

    private boolean tryRigth() {
        switch (mapper.getDirection()) {
            case 0:
                if (mapper.testAlreadyStay(mapper.getX(), mapper.getY() - 1)) {
                    return true;
                } else {
                    return false;
                }
            case 1:
                if (mapper.testAlreadyStay(mapper.getX() + 1, mapper.getY())) {
                    return true;
                } else {
                    return false;
                }
            case 2:
                if (mapper.testAlreadyStay(mapper.getX(), mapper.getY() + 1)) {
                    return true;
                } else {
                    return false;
                }
            case 3:
                if (mapper.testAlreadyStay(mapper.getX() - 1, mapper.getY())) {
                    return true;
                } else {
                    return false;
                }
        }
        return false;
    }
}
